#include <AStar.h>
#include <m_heap.h>
#include <time.h>
#include <unistd.h>
#define ins_func default_function
AStar::AStar(/* args */Mitrix_M_Map* m)
{
    this->M_Map=m;
}

AStar::~AStar()
{
}

void AStar:: findPath(m_position start,m_position end_p){
    m_position** parents=this->M_Map->generateParentM_Map();
    this->M_Map->setStart(start);
    this->M_Map->setEnd(end_p);
    M_Map->print();
    int step=0;
    M_Heap<m_position> heap(true);
    m_position now=start;
    while(now.col!=end_p.col||now.row!=end_p.row){
        system("clear");
        this->M_Map->print();
        step=step+1;
        m_position up=this->M_Map->move(now,e_move_up);
        if(this->M_Map->isValid(up)&&!M_Map->isVisited(up)){
            heap_data<m_position> d;
            d.data=up;
            d.key=step+ins_func(up,end_p);
            heap.addData(d);
            parents[up.row][up.col]=now;
            this->M_Map->setVisit(up);
        }
        m_position down=this->M_Map->move(now,e_move_down);
        if(this->M_Map->isValid(down)&&!M_Map->isVisited(down)){
            heap_data<m_position> d;
            d.data=down;
            d.key=step+ins_func(down,end_p);
            heap.addData(d);
            parents[down.row][down.col]=now;
            this->M_Map->setVisit(down);
        }
        m_position left=this->M_Map->move(now,e_move_left);
        if(this->M_Map->isValid(left)&&!M_Map->isVisited(left)){
            heap_data<m_position> d;
            d.data=left;
            d.key=step+ins_func(left,end_p);
            heap.addData(d);
            parents[left.row][left.col]=now;
            this->M_Map->setVisit(left);
        }
        m_position right=this->M_Map->move(now,e_move_right);
        if(this->M_Map->isValid(right)&&!M_Map->isVisited(right)){
            heap_data<m_position> d;
            d.data=right;
            d.key=step+ins_func(right,end_p);
            heap.addData(d);
            parents[right.row][right.col]=now;
            this->M_Map->setVisit(right);
        }
        now=heap.pop().data;
        // this->M_Map->setPath(now);
        usleep(300*1000);
    }
    m_position parent=parents[end_p.row][end_p.col];
    while(parent.col!=start.col||parent.row!=start.row){
        this->M_Map->setPath(parent);
        parent=parents[parent.row][parent.col];
    }
    this->M_Map->print();
}

int main(){
    Mitrix_M_Map m(10,10);
    m.addBlock({2,4});
    m.addBlock({3,4});
    m.addBlock({4,4});
    m.addBlock({5,4});
    // m.addBlock({6,4});
    m.addBlock({7,4});
    // m.print();
    AStar astar(&m);
    // astar.ins_func=default_function();
    astar.findPath({2,1},{6,7});
    
}